/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
/
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

namespace cv {

inline float sqr(uchar a) { return CV_8TO32F_SQR(a); }
inline float sqr(float a) { return a * a; }

inline double sqr(double a) { return a * a; }

inline Vec3f sqr(const Vec3b& a) {
    return Vec3f(CV_8TO32F_SQR(a[0]), CV_8TO32F_SQR(a[1]), CV_8TO32F_SQR(a[2]));
}
inline Vec3f sqr(const Vec3f& a) {
    return Vec3f(a[0] * a[0], a[1] * a[1], a[2] * a[2]);
}
inline Vec3d sqr(const Vec3d& a) {
    return Vec3d(a[0] * a[0], a[1] * a[1], a[2] * a[2]);
}
inline float multiply(uchar a, uchar b) { return CV_8TO32F(a) * CV_8TO32F(b); }
inline float multiply(float a, float b) { return a * b; }
inline double multiply(double a, double b) { return a * b; }
inline Vec3f multiply(const Vec3b& a, const Vec3b& b) {
    return Vec3f(
               CV_8TO32F(a[0]) * CV_8TO32F(b[0]),
               CV_8TO32F(a[1]) * CV_8TO32F(b[1]),
               CV_8TO32F(a[2]) * CV_8TO32F(b[2]));
}
inline Vec3f multiply(const Vec3f& a, const Vec3f& b) {
    return Vec3f(a[0] * b[0], a[1] * b[1], a[2] * b[2]);
}
inline Vec3d multiply(const Vec3d& a, const Vec3d& b) {
    return Vec3d(a[0] * b[0], a[1] * b[1], a[2] * b[2]);
}

inline float addw(uchar a, float alpha, float b, float beta) {
    return b * beta + CV_8TO32F(a) * alpha;
}
inline float addw(float a, float alpha, float b, float beta) {
    return b * beta + a * alpha;
}
inline double addw(uchar a, double alpha, double b, double beta) {
    return b * beta + CV_8TO32F(a) * alpha;
}
inline double addw(float a, double alpha, double b, double beta) {
    return b * beta + a * alpha;
}
inline double addw(double a, double alpha, double b, double beta) {
    return b * beta + a * alpha;
}

inline Vec3f addw(const Vec3b& a, float alpha, const Vec3f& b, float beta) {
    return Vec3f(b[0] * beta + CV_8TO32F(a[0]) * alpha,
                 b[1] * beta + CV_8TO32F(a[1]) * alpha,
                 b[2] * beta + CV_8TO32F(a[2]) * alpha);
}
inline Vec3f addw(const Vec3f& a, float alpha, const Vec3f& b, float beta) {
    return Vec3f(b[0] * beta + a[0] * alpha, b[1] * beta + a[1] * alpha, b[2] * beta + a[2] * alpha);
}
inline Vec3d addw(const Vec3b& a, double alpha, const Vec3d& b, double beta) {
    return Vec3d(b[0] * beta + CV_8TO32F(a[0]) * alpha,
                 b[1] * beta + CV_8TO32F(a[1]) * alpha,
                 b[2] * beta + CV_8TO32F(a[2]) * alpha);
}
inline Vec3d addw(const Vec3f& a, double alpha, const Vec3d& b, double beta) {
    return Vec3d(b[0] * beta + a[0] * alpha, b[1] * beta + a[1] * alpha, b[2] * beta + a[2] * alpha);
}
inline Vec3d addw(const Vec3d& a, double alpha, const Vec3d& b, double beta) {
    return Vec3d(b[0] * beta + a[0] * alpha, b[1] * beta + a[1] * alpha, b[2] * beta + a[2] * alpha);
}

template<typename T, typename AT> void
acc_(const Mat& _src, Mat& _dst) {
    Size size = _src.size();
    size.width *= _src.channels();

    if (_src.isContinuous() && _dst.isContinuous()) {
        size.width *= size.height;
        size.height = 1;
    }

    int i, j;
    for (i = 0; i < size.height; i++) {
        const T* src = (const T*)(_src.data + _src.step * i);
        AT* dst = (AT*)(_dst.data + _dst.step * i);

        for (j = 0; j <= size.width - 4; j += 4) {
            AT t0 = dst[j] + src[j], t1 = dst[j + 1] + src[j + 1];
            dst[j] = t0; dst[j + 1] = t1;
            t0 = dst[j + 2] + src[j + 2]; t1 = dst[j + 3] + src[j + 3];
            dst[j + 2] = t0; dst[j + 3] = t1;
        }

        for (; j < size.width; j++) {
            dst[j] += src[j];
        }
    }
}


template<typename T, typename AT> void
accSqr_(const Mat& _src, Mat& _dst) {
    Size size = _src.size();
    size.width *= _src.channels();

    if (_src.isContinuous() && _dst.isContinuous()) {
        size.width *= size.height;
        size.height = 1;
    }

    int i, j;
    for (i = 0; i < size.height; i++) {
        const T* src = (const T*)(_src.data + _src.step * i);
        AT* dst = (AT*)(_dst.data + _dst.step * i);

        for (j = 0; j <= size.width - 4; j += 4) {
            AT t0 = dst[j] + sqr(src[j]), t1 = dst[j + 1] + sqr(src[j + 1]);
            dst[j] = t0; dst[j + 1] = t1;
            t0 = dst[j + 2] + sqr(src[j + 2]); t1 = dst[j + 3] + sqr(src[j + 3]);
            dst[j + 2] = t0; dst[j + 3] = t1;
        }

        for (; j < size.width; j++) {
            dst[j] += sqr(src[j]);
        }
    }
}


template<typename T, typename AT> void
accProd_(const Mat& _src1, const Mat& _src2, Mat& _dst) {
    Size size = _src1.size();
    size.width *= _src1.channels();

    if (_src1.isContinuous() && _src2.isContinuous() && _dst.isContinuous()) {
        size.width *= size.height;
        size.height = 1;
    }

    int i, j;
    for (i = 0; i < size.height; i++) {
        const T* src1 = (const T*)(_src1.data + _src1.step * i);
        const T* src2 = (const T*)(_src2.data + _src2.step * i);
        AT* dst = (AT*)(_dst.data + _dst.step * i);

        for (j = 0; j <= size.width - 4; j += 4) {
            AT t0, t1;
            t0 = dst[j] + multiply(src1[j], src2[j]);
            t1 = dst[j + 1] + multiply(src1[j + 1], src2[j + 1]);
            dst[j] = t0; dst[j + 1] = t1;
            t0 = dst[j + 2] + multiply(src1[j + 2], src2[j + 2]);
            t1 = dst[j + 3] + multiply(src1[j + 3], src2[j + 3]);
            dst[j + 2] = t0; dst[j + 3] = t1;
        }

        for (; j < size.width; j++) {
            dst[j] += multiply(src1[j], src2[j]);
        }
    }
}


template<typename T, typename AT> void
accW_(const Mat& _src, Mat& _dst, double _alpha) {
    AT alpha = (AT)_alpha, beta = (AT)(1 - _alpha);
    Size size = _src.size();
    size.width *= _src.channels();

    if (_src.isContinuous() && _dst.isContinuous()) {
        size.width *= size.height;
        size.height = 1;
    }

    int i, j;
    for (i = 0; i < size.height; i++) {
        const T* src = (const T*)(_src.data + _src.step * i);
        AT* dst = (AT*)(_dst.data + _dst.step * i);

        for (j = 0; j <= size.width - 4; j += 4) {
            AT t0, t1;
            t0 = addw(src[j], alpha, dst[j], beta);
            t1 = addw(src[j + 1], alpha, dst[j + 1], beta);
            dst[j] = t0; dst[j + 1] = t1;
            t0 = addw(src[j + 2], alpha, dst[j + 2], beta);
            t1 = addw(src[j + 3], alpha, dst[j + 3], beta);
            dst[j + 2] = t0; dst[j + 3] = t1;
        }

        for (; j < size.width; j++) {
            dst[j] = addw(src[j], alpha, dst[j], beta);
        }
    }
}


template<typename T, typename AT> void
accMask_(const Mat& _src, Mat& _dst, const Mat& _mask) {
    Size size = _src.size();

    if (_src.isContinuous() && _dst.isContinuous() && _mask.isContinuous()) {
        size.width *= size.height;
        size.height = 1;
    }

    int i, j;
    for (i = 0; i < size.height; i++) {
        const T* src = (const T*)(_src.data + _src.step * i);
        AT* dst = (AT*)(_dst.data + _dst.step * i);
        const uchar* mask = _mask.data + _mask.step * i;

        for (j = 0; j < size.width; j++)
            if (mask[j]) {
                dst[j] += src[j];
            }
    }
}


template<typename T, typename AT> void
accSqrMask_(const Mat& _src, Mat& _dst, const Mat& _mask) {
    Size size = _src.size();

    if (_src.isContinuous() && _dst.isContinuous() && _mask.isContinuous()) {
        size.width *= size.height;
        size.height = 1;
    }

    int i, j;
    for (i = 0; i < size.height; i++) {
        const T* src = (const T*)(_src.data + _src.step * i);
        AT* dst = (AT*)(_dst.data + _dst.step * i);
        const uchar* mask = _mask.data + _mask.step * i;

        for (j = 0; j < size.width; j++)
            if (mask[j]) {
                dst[j] += sqr(src[j]);
            }
    }
}


template<typename T, typename AT> void
accProdMask_(const Mat& _src1, const Mat& _src2, Mat& _dst, const Mat& _mask) {
    Size size = _src1.size();

    if (_src1.isContinuous() && _src2.isContinuous() &&
            _dst.isContinuous() && _mask.isContinuous()) {
        size.width *= size.height;
        size.height = 1;
    }

    int i, j;
    for (i = 0; i < size.height; i++) {
        const T* src1 = (const T*)(_src1.data + _src1.step * i);
        const T* src2 = (const T*)(_src2.data + _src2.step * i);
        AT* dst = (AT*)(_dst.data + _dst.step * i);
        const uchar* mask = _mask.data + _mask.step * i;

        for (j = 0; j < size.width; j++)
            if (mask[j]) {
                dst[j] += multiply(src1[j], src2[j]);
            }
    }
}


template<typename T, typename AT> void
accWMask_(const Mat& _src, Mat& _dst, double _alpha, const Mat& _mask) {
    typedef typename DataType<AT>::channel_type AT1;
    AT1 alpha = (AT1)_alpha, beta = (AT1)(1 - _alpha);
    Size size = _src.size();

    if (_src.isContinuous() && _dst.isContinuous() && _mask.isContinuous()) {
        size.width *= size.height;
        size.height = 1;
    }

    int i, j;
    for (i = 0; i < size.height; i++) {
        const T* src = (const T*)(_src.data + _src.step * i);
        AT* dst = (AT*)(_dst.data + _dst.step * i);
        const uchar* mask = _mask.data + _mask.step * i;

        for (j = 0; j < size.width; j++)
            if (mask[j]) {
                dst[j] = addw(src[j], alpha, dst[j], beta);
            }
    }
}


typedef void (*AccFunc)(const Mat&, Mat&);
typedef void (*AccMaskFunc)(const Mat&, Mat&, const Mat&);
typedef void (*AccProdFunc)(const Mat&, const Mat&, Mat&);
typedef void (*AccProdMaskFunc)(const Mat&, const Mat&, Mat&, const Mat&);
typedef void (*AccWFunc)(const Mat&, Mat&, double);
typedef void (*AccWMaskFunc)(const Mat&, Mat&, double, const Mat&);

void accumulate(const Mat& src, Mat& dst, const Mat& mask) {
    CV_Assert(dst.size() == src.size() && dst.channels() == src.channels());

    if (!mask.data) {
        AccFunc func = 0;
        if (src.depth() == CV_8U && dst.depth() == CV_32F) {
            func = acc_<uchar, float>;
        } else if (src.depth() == CV_8U && dst.depth() == CV_64F) {
            func = acc_<uchar, double>;
        } else if (src.depth() == CV_32F && dst.depth() == CV_32F) {
            func = acc_<float, float>;
        } else if (src.depth() == CV_32F && dst.depth() == CV_64F) {
            func = acc_<float, double>;
        } else if (src.depth() == CV_64F && dst.depth() == CV_64F) {
            func = acc_<double, double>;
        } else {
            CV_Error(CV_StsUnsupportedFormat, "");
        }

        func(src, dst);
    } else {
        CV_Assert(mask.size() == src.size() && mask.type() == CV_8UC1);

        AccMaskFunc func = 0;
        if (src.type() == CV_8UC1 && dst.type() == CV_32FC1) {
            func = accMask_<uchar, float>;
        } else if (src.type() == CV_8UC3 && dst.type() == CV_32FC3) {
            func = accMask_<Vec3b, Vec3f>;
        } else if (src.type() == CV_8UC1 && dst.type() == CV_64FC1) {
            func = accMask_<uchar, double>;
        } else if (src.type() == CV_8UC3 && dst.type() == CV_64FC3) {
            func = accMask_<Vec3b, Vec3d>;
        } else if (src.type() == CV_32FC1 && dst.type() == CV_32FC1) {
            func = accMask_<float, float>;
        } else if (src.type() == CV_32FC3 && dst.type() == CV_32FC3) {
            func = accMask_<Vec3f, Vec3f>;
        } else if (src.type() == CV_32FC1 && dst.type() == CV_64FC1) {
            func = accMask_<float, double>;
        } else if (src.type() == CV_32FC3 && dst.type() == CV_64FC3) {
            func = accMask_<Vec3f, Vec3d>;
        } else if (src.type() == CV_64FC1 && dst.type() == CV_64FC1) {
            func = accMask_<double, double>;
        } else if (src.type() == CV_64FC3 && dst.type() == CV_64FC3) {
            func = accMask_<Vec3d, Vec3d>;
        } else {
            CV_Error(CV_StsUnsupportedFormat, "");
        }

        func(src, dst, mask);
    }
}


void accumulateSquare(const Mat& src, Mat& dst, const Mat& mask) {
    CV_Assert(dst.size() == src.size() && dst.channels() == src.channels());

    if (!mask.data) {
        AccFunc func = 0;
        if (src.depth() == CV_8U && dst.depth() == CV_32F) {
            func = accSqr_<uchar, float>;
        } else if (src.depth() == CV_8U && dst.depth() == CV_64F) {
            func = accSqr_<uchar, double>;
        } else if (src.depth() == CV_32F && dst.depth() == CV_32F) {
            func = accSqr_<float, float>;
        } else if (src.depth() == CV_32F && dst.depth() == CV_64F) {
            func = accSqr_<float, double>;
        } else if (src.depth() == CV_64F && dst.depth() == CV_64F) {
            func = accSqr_<double, double>;
        } else {
            CV_Error(CV_StsUnsupportedFormat, "");
        }

        func(src, dst);
    } else {
        CV_Assert(mask.size() == src.size() && mask.type() == CV_8UC1);

        AccMaskFunc func = 0;
        if (src.type() == CV_8UC1 && dst.type() == CV_32FC1) {
            func = accSqrMask_<uchar, float>;
        } else if (src.type() == CV_8UC3 && dst.type() == CV_32FC3) {
            func = accSqrMask_<Vec3b, Vec3f>;
        } else if (src.type() == CV_8UC1 && dst.type() == CV_64FC1) {
            func = accSqrMask_<uchar, double>;
        } else if (src.type() == CV_8UC3 && dst.type() == CV_64FC3) {
            func = accSqrMask_<Vec3b, Vec3d>;
        } else if (src.type() == CV_32FC1 && dst.type() == CV_32FC1) {
            func = accSqrMask_<float, float>;
        } else if (src.type() == CV_32FC3 && dst.type() == CV_32FC3) {
            func = accSqrMask_<Vec3f, Vec3f>;
        } else if (src.type() == CV_32FC1 && dst.type() == CV_64FC1) {
            func = accSqrMask_<float, double>;
        } else if (src.type() == CV_32FC3 && dst.type() == CV_64FC3) {
            func = accSqrMask_<Vec3f, Vec3d>;
        } else if (src.type() == CV_64FC1 && dst.type() == CV_64FC1) {
            func = accSqrMask_<double, double>;
        } else if (src.type() == CV_64FC3 && dst.type() == CV_64FC3) {
            func = accSqrMask_<Vec3d, Vec3d>;
        } else {
            CV_Error(CV_StsUnsupportedFormat, "");
        }

        func(src, dst, mask);
    }
}


void accumulateProduct(const Mat& src1, const Mat& src2, Mat& dst, const Mat& mask) {
    CV_Assert(dst.size() == src1.size() && dst.channels() == src1.channels() &&
              src1.size() == src2.size() && src1.type() == src2.type());

    if (!mask.data) {
        AccProdFunc func = 0;
        if (src1.depth() == CV_8U && dst.depth() == CV_32F) {
            func = accProd_<uchar, float>;
        } else if (src1.depth() == CV_8U && dst.depth() == CV_64F) {
            func = accProd_<uchar, double>;
        } else if (src1.depth() == CV_32F && dst.depth() == CV_32F) {
            func = accProd_<float, float>;
        } else if (src1.depth() == CV_32F && dst.depth() == CV_64F) {
            func = accProd_<float, double>;
        } else if (src1.depth() == CV_64F && dst.depth() == CV_64F) {
            func = accProd_<double, double>;
        } else {
            CV_Error(CV_StsUnsupportedFormat, "");
        }

        func(src1, src2, dst);
    } else {
        CV_Assert(mask.size() == src1.size() && mask.type() == CV_8UC1);

        AccProdMaskFunc func = 0;
        if (src1.type() == CV_8UC1 && dst.type() == CV_32FC1) {
            func = accProdMask_<uchar, float>;
        } else if (src1.type() == CV_8UC3 && dst.type() == CV_32FC3) {
            func = accProdMask_<Vec3b, Vec3f>;
        } else if (src1.type() == CV_8UC1 && dst.type() == CV_64FC1) {
            func = accProdMask_<uchar, double>;
        } else if (src1.type() == CV_8UC3 && dst.type() == CV_64FC3) {
            func = accProdMask_<Vec3b, Vec3d>;
        } else if (src1.type() == CV_32FC1 && dst.type() == CV_32FC1) {
            func = accProdMask_<float, float>;
        } else if (src1.type() == CV_32FC3 && dst.type() == CV_32FC3) {
            func = accProdMask_<Vec3f, Vec3f>;
        } else if (src1.type() == CV_32FC1 && dst.type() == CV_64FC1) {
            func = accProdMask_<float, double>;
        } else if (src1.type() == CV_32FC3 && dst.type() == CV_64FC3) {
            func = accProdMask_<Vec3f, Vec3d>;
        } else if (src1.type() == CV_64FC1 && dst.type() == CV_64FC1) {
            func = accProdMask_<double, double>;
        } else if (src1.type() == CV_64FC3 && dst.type() == CV_64FC3) {
            func = accProdMask_<Vec3d, Vec3d>;
        } else {
            CV_Error(CV_StsUnsupportedFormat, "");
        }

        func(src1, src2, dst, mask);
    }
}


void accumulateWeighted(const Mat& src, Mat& dst, double alpha, const Mat& mask) {
    CV_Assert(dst.size() == src.size() && dst.channels() == src.channels());

    if (!mask.data) {
        AccWFunc func = 0;
        if (src.depth() == CV_8U && dst.depth() == CV_32F) {
            func = accW_<uchar, float>;
        } else if (src.depth() == CV_8U && dst.depth() == CV_64F) {
            func = accW_<uchar, double>;
        } else if (src.depth() == CV_32F && dst.depth() == CV_32F) {
            func = accW_<float, float>;
        } else if (src.depth() == CV_32F && dst.depth() == CV_64F) {
            func = accW_<float, double>;
        } else if (src.depth() == CV_64F && dst.depth() == CV_64F) {
            func = accW_<double, double>;
        } else {
            CV_Error(CV_StsUnsupportedFormat, "");
        }

        func(src, dst, alpha);
    } else {
        CV_Assert(mask.size() == src.size() && mask.type() == CV_8UC1);

        AccWMaskFunc func = 0;
        if (src.type() == CV_8UC1 && dst.type() == CV_32FC1) {
            func = accWMask_<uchar, float>;
        } else if (src.type() == CV_8UC3 && dst.type() == CV_32FC3) {
            func = accWMask_<Vec3b, Vec3f>;
        } else if (src.type() == CV_8UC1 && dst.type() == CV_64FC1) {
            func = accWMask_<uchar, double>;
        } else if (src.type() == CV_8UC3 && dst.type() == CV_64FC3) {
            func = accWMask_<Vec3b, Vec3d>;
        } else if (src.type() == CV_32FC1 && dst.type() == CV_32FC1) {
            func = accWMask_<float, float>;
        } else if (src.type() == CV_32FC3 && dst.type() == CV_32FC3) {
            func = accWMask_<Vec3f, Vec3f>;
        } else if (src.type() == CV_32FC1 && dst.type() == CV_64FC1) {
            func = accWMask_<float, double>;
        } else if (src.type() == CV_32FC3 && dst.type() == CV_64FC3) {
            func = accWMask_<Vec3f, Vec3d>;
        } else if (src.type() == CV_64FC1 && dst.type() == CV_64FC1) {
            func = accWMask_<double, double>;
        } else if (src.type() == CV_64FC3 && dst.type() == CV_64FC3) {
            func = accWMask_<Vec3d, Vec3d>;
        } else {
            CV_Error(CV_StsUnsupportedFormat, "");
        }

        func(src, dst, alpha, mask);
    }
}

}


CV_IMPL void
cvAcc(const void* arr, void* sumarr, const void* maskarr) {
    cv::Mat src = cv::cvarrToMat(arr), dst = cv::cvarrToMat(sumarr), mask;
    if (maskarr) {
        mask = cv::cvarrToMat(maskarr);
    }
    cv::accumulate(src, dst, mask);
}

CV_IMPL void
cvSquareAcc(const void* arr, void* sumarr, const void* maskarr) {
    cv::Mat src = cv::cvarrToMat(arr), dst = cv::cvarrToMat(sumarr), mask;
    if (maskarr) {
        mask = cv::cvarrToMat(maskarr);
    }
    cv::accumulateSquare(src, dst, mask);
}

CV_IMPL void
cvMultiplyAcc(const void* arr1, const void* arr2,
              void* sumarr, const void* maskarr) {
    cv::Mat src1 = cv::cvarrToMat(arr1), src2 = cv::cvarrToMat(arr2);
    cv::Mat dst = cv::cvarrToMat(sumarr), mask;
    if (maskarr) {
        mask = cv::cvarrToMat(maskarr);
    }
    cv::accumulateProduct(src1, src2, dst, mask);
}

CV_IMPL void
cvRunningAvg(const void* arr, void* sumarr, double alpha, const void* maskarr) {
    cv::Mat src = cv::cvarrToMat(arr), dst = cv::cvarrToMat(sumarr), mask;
    if (maskarr) {
        mask = cv::cvarrToMat(maskarr);
    }
    cv::accumulateWeighted(src, dst, alpha, mask);
}

/* End of file. */
